import heapq
import math
class Node:
   def __init__(self, x, y, cost, parent=None):
       self.x = x
       self.y = y
       self.cost = cost
       self.parent = parent
   def __lt__(self, other):
       return self.cost < other.cost
def heuristic(a, b):
   return math.sqrt((a[0] - b[0])**2 + (a[1] - b[1])**2)
def a_star_search(start, goal, obstacles):
   open_list = []
   closed_set = set()
   start_node = Node(start[0], start[1], 0)
   goal_node = Node(goal[0], goal[1], 0)
   heapq.heappush(open_list, (0, start_node))
   while open_list:
       _, current_node = heapq.heappop(open_list)
       if (current_node.x, current_node.y) == (goal_node.x, goal_node.y):
           path = []
           while current_node:
               path.append((current_node.x, current_node.y))
               current_node = current_node.parent
           return path[::-1]
       closed_set.add((current_node.x, current_node.y))
       for dx, dy in [(-1, 0), (1, 0), (0, -1), (0, 1)]:
           neighbor_x, neighbor_y = current_node.x + dx, current_node.y + dy
           if (neighbor_x, neighbor_y) in obstacles or (neighbor_x, neighbor_y) in closed_set:
               continue
           neighbor_cost = current_node.cost + 1
           neighbor_heuristic = heuristic((neighbor_x, neighbor_y), (goal_node.x, goal_node.y))
           neighbor_node = Node(neighbor_x, neighbor_y, neighbor_cost + neighbor_heuristic, current_node)
           heapq.heappush(open_list, (neighbor_cost + neighbor_heuristic, neighbor_node))
   return None
# 示例运行
start = (0, 0)
goal = (5, 5)
obstacles = {(2, 2), (3, 3), (4, 4)}
path = a_star_search(start, goal, obstacles)
print("路径:", path)